#include "pid_ctrl.h"


static PID_t TMP_PID = {0};


void InitTMP_PID(float kp, float ki, float kd)
{
    TMP_PID.kp = kp;
    TMP_PID.ki = ki;
    TMP_PID.kd = kd;
}

void SetPID_Target(float tar, float out_max)
{
    TMP_PID.tar = tar;
    TMP_PID.out_max = out_max;
}

float PID_Ctrl(float pre, uint16_t TimeTick)
{
    TMP_PID.pre = pre;
    TMP_PID.last_bias = TMP_PID.bias;

    TMP_PID.bias = TMP_PID.tar - TMP_PID.pre;
    TMP_PID.sum_bias += TMP_PID.bias;
    TMP_PID.out = TMP_PID.kp*TMP_PID.bias + TMP_PID.kd*(TMP_PID.bias-TMP_PID.last_bias)/TimeTick + TMP_PID.ki*TMP_PID.sum_bias;

    if(TMP_PID.out > TMP_PID.out_max)
    {
        TMP_PID.out = TMP_PID.out_max;
    }
    else if(TMP_PID.out < -TMP_PID.out_max)
    {
        TMP_PID.out = -TMP_PID.out_max;
    }
    return TMP_PID.out;
}



